#include "imgstitcher.h"

ImgStitcher::ImgStitcher()
{

}

// 静态成员必须在类外初始化，否则会出现 undefined reference to ImgStitcher::m_corners  的错误
ImgStitcher::four_corners_t ImgStitcher::m_corners;

void ImgStitcher::CalcCorners(const Mat& H, const Mat& src)
{
    double v2[] = { 0, 0, 1 };//左上角
    double v1[3];//变换后的坐标值
    Mat V2 = Mat(3, 1, CV_64FC1, v2);  //列向量
    Mat V1 = Mat(3, 1, CV_64FC1, v1);  //列向量

    V1 = H * V2;
    //左上角(0,0,1)
    m_corners.left_top.x = v1[0] / v1[2];
    m_corners.left_top.y = v1[1] / v1[2];

    //左下角(0,src.rows,1)
    v2[0] = 0;
    v2[1] = src.rows;
    v2[2] = 1;
    V2 = Mat(3, 1, CV_64FC1, v2);  //列向量
    V1 = Mat(3, 1, CV_64FC1, v1);  //列向量
    V1 = H * V2;
    m_corners.left_bottom.x = v1[0] / v1[2];
    m_corners.left_bottom.y = v1[1] / v1[2];

    //右上角(src.cols,0,1)
    v2[0] = src.cols;
    v2[1] = 0;
    v2[2] = 1;
    V2 = Mat(3, 1, CV_64FC1, v2);  //列向量
    V1 = Mat(3, 1, CV_64FC1, v1);  //列向量
    V1 = H * V2;
    m_corners.right_top.x = v1[0] / v1[2];
    m_corners.right_top.y = v1[1] / v1[2];

    //右下角(src.cols,src.rows,1)
    v2[0] = src.cols;
    v2[1] = src.rows;
    v2[2] = 1;
    V2 = Mat(3, 1, CV_64FC1, v2);  //列向量
    V1 = Mat(3, 1, CV_64FC1, v1);  //列向量
    V1 = H * V2;
    m_corners.right_bottom.x = v1[0] / v1[2];
    m_corners.right_bottom.y = v1[1] / v1[2];
}

cv::Mat ImgStitcher::stitchTwoImg(cv::Mat& image1, cv::Mat& image2) {
    /*
    step1:特征检测器
    */
    cv::Ptr<cv::xfeatures2d::SURF> detector;
    detector = cv::xfeatures2d::SURF::create(800);    // 800为海塞矩阵阈值，越大越精准
    /*
    -----SURF----
    cv::Ptr<cv::xfeatures2d::SURF> detector;
    detector = cv::xfeatures2d::SURF::create(800);  //800为海塞矩阵阈值，越大越精准
    -----SIFT-----
    cv::Ptr<cv::xfeatures2d::SIFT> detector;
    detector = cv::xfeatures2d::SIFT::create(800);//800为保留的点数
    -----ORB------
    cv::Ptr<cv::ORB> detector;
    detector  = cv::ORB::create(800);//保留点数
    -----STAR-----
    cv::Ptr<cv::xfeatures2d::StarDetector> detector;
    detector = cv::xfeatures2d::StarDetector::create();
    -----MSD-----
    cv::Ptr<cv::xfeatures2d::MSDDetector> detector;
    detector = cv::xfeatures2d::MSDDetector::create();
    */
    std::vector <cv::KeyPoint> keyPoint1;
    std::vector <cv::KeyPoint> keyPoint2;
    detector->detect(image1,keyPoint1,cv::noArray());
    detector->detect(image2, keyPoint2, cv::noArray());

    /*
    step2:描述子提取器
    */
    cv::Ptr<cv::xfeatures2d::SURF> Extractor;
    Extractor = cv::xfeatures2d::SURF::create(800);
    /*
       以下都是xfeature2d中的提取器
    -----SURF-----
    -----SIFT-----
    -----LUCID----
    -----BriefDescriptorExtractor----
    -----VGG-----
    -----BoostDesc-----
    */
    cv::Mat descriptor1, descriptor2;
    Extractor->compute(image1, keyPoint1, descriptor1);
    Extractor->compute(image2, keyPoint2, descriptor2);

    /*
    step3:匹配器
    */
    // 暴力匹配特征点
    cv::BFMatcher matcher;
    std::vector<cv::DMatch> matches;
    std::vector<cv::DMatch> good_matches;
    matcher.match(descriptor1, descriptor2, matches);
    std::sort(matches.begin(), matches.end());
    // 选择距离最小的50个匹配对存放到good_matches里
    int ptsPairs = std::min(50, (int)(matches.size() * 0.15));
    //qDebug() << "匹配点数为" << ptsPairs;
    for (int i = 0; i < ptsPairs; i++)
    {
        good_matches.push_back(matches[i]);
    }
    cv::Mat result;
    // 绘制匹配点
    cv::drawMatches(image1, keyPoint1,
        image2, keyPoint2,
        good_matches, result,
        cv::Scalar::all(-1), cv::Scalar::all(-1),
        std::vector<char>(),
        cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
    //cv::imshow("result", result);

    /*
    step4:根据匹配的特征点对计算变换矩阵，并且将图像与矩阵运算
    */
    vector<Point2f> imagePoints1, imagePoints2;
    for (int i = 0; i < good_matches.size(); i++)
    {
        imagePoints1.push_back(keyPoint1[good_matches[i].queryIdx].pt);
        imagePoints2.push_back(keyPoint2[good_matches[i].trainIdx].pt);
    }
    //获取图像2到图像1的投影映射矩阵 尺寸为3*3
    Mat homo = findHomography(imagePoints2, imagePoints1, RANSAC);
    ////也可以使用getPerspectiveTransform方法获得透视变换矩阵，不过要求只能有4个点，效果稍差
    //Mat homo=getPerspectiveTransform(imagePoints1,imagePoints2);
    //qDebug() << "变换矩阵为：" << cv::format(homo, Formatter::FMT_CSV); // 输出映射矩阵
    //图像配准
    Mat imageTransformed, imageTransform2;
    CalcCorners(homo, image2);
    warpPerspective(image2, imageTransformed, homo, Size(MAX(m_corners.right_top.x, m_corners.right_bottom.x), image2.rows));
    //warpPerspective(image01, imageTransform2, adjustMat*homo, Size(image02.cols*1.3, image02.rows*1.8));
    //imshow("transformedImage", imageTransformed);

    /*
    step5:图像融合
    */
    // 创建拼接后的图,需提前计算图的大小
    int dst_width = imageTransformed.cols;
    int dst_height = imageTransformed.rows;
    Mat dst(dst_height, dst_width, CV_8UC1);
    dst.setTo(0);
    imageTransformed.copyTo(dst(Rect(0, 0, imageTransformed.cols, imageTransformed.rows)));
    image1.copyTo(dst(Rect(0, 0, image1.cols, image1.rows)));
    //imshow("b_dst", dst);
    //qDebug() << "拼接后的图像尺寸为：" << dst.cols << " * " << dst.rows;
    return dst;
}

cv::Mat ImgStitcher::stitchImgs(deque<cv::Mat>& imgs) {
    cv::Mat outImg = imgs[0];
    for (int i = 1; i < imgs.size(); ++i) {
        outImg = stitchTwoImg(outImg, imgs[i]);
    }
    return outImg;
}

int ImgStitcher::evaluateRes(cv::Mat& tarImg, cv::Mat& refImg, cv::Mat& outImg) {
    Mat absImg;
    cv::absdiff(tarImg, refImg, absImg);
    cv::threshold(absImg, outImg, 1, 255, cv::THRESH_BINARY);
    //imshow("thresholdRes", outImg);
    int ret = 0;
    for (int r = 0; r < outImg.rows; ++r) {
        for (int c = 0; c < outImg.cols; ++c) {
            if (outImg.ptr<uchar>(r)[c] > 0) ++ret;
        }
    }
    return ret;
}

cv::Mat ImgStitcher::matchImage(cv::Mat& totalImg, cv::Mat& tarImg) {
    cv::Mat result, outImg;
    cv::matchTemplate(totalImg, tarImg, result, cv::TM_CCOEFF_NORMED);
    imshow("matchRes", result);
    // 定位系数矩阵中的最大值位置
    double maxVal, minVal;
    cv::Point maxLoc, minLoc;
    cv::minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc);
    // 截取目标区域
    cv::Rect rect(maxLoc.x, maxLoc.y, tarImg.cols, tarImg.rows);
    outImg = totalImg(rect);
    imshow("outImg", outImg);
    // 在大图像中绘制匹配到的区域
    cv::Mat tmp;
    cv::cvtColor(totalImg, tmp, cv::COLOR_GRAY2RGB);
    cv::rectangle(tmp, rect, cv::Scalar(255, 0, 0), 2);
    imshow("totalImg", tmp);
    return outImg;
}
